#include "control.h"


/**
 * @brief  返回目标角度与当前角度的锐角差值，带符号， +:顺时针转动, -:逆时针转动
 * @note
 * @param  cur_lat:
 * @param  cur_lon:
 * @param  des_lat:
 * @param  des_lon:
 * @retval
 */
double get_direction_err(double des_lat, double des_lon, double cur_lat, double cur_lon)
{
	double cur_dir;
	double err;

	get_heading_angle(&cur_dir);
	cur_dir = fmod(cur_dir + 90, 360);


	double tar_dir = get_target_direction(des_lat, des_lon, cur_lat, cur_lon); //获取目标方向
    printf("tar_dir:%f\tcur_dir:%f\n", tar_dir, cur_dir);
	err = tar_dir - cur_dir;

	// 顺时针为正方向
	// err 		 : -180 <= err <= 180
	// err - 360 : err > 180
	// err + 360 : err < -180
	err = fabs(err) <= 180 ? err : err - (err / fabs(err)) * 360;

	return err;
}


double kd = 50;		//距离P
double ka = 15;		//角度P
double ko = 80;		//障碍物P

int tank_control(double cur_lat, double cur_lon, double des_lat, double des_lon)
{
	double l_vel = 0;
	double r_vel = 0;
	double dis_err = 0;
	double ang_err = 0;
	double obs_err = 0;

	dis_err = get_distance_err(des_lat, des_lon, cur_lat, cur_lon);		//获取距离差
	ang_err = get_direction_err(des_lat, des_lon, cur_lat, cur_lon);	//获取角度差
	obs_err = get_radar_info();											//获取障碍物的信息

	if(obs_err < -1)
	{
		set_speed(0, 0);
		printf("radar err\n");

		return 0;
	}

	if(fabs(dis_err) < 0.5)			//到达目标点
	{
		set_speed(0, 0);

		return 1;
	}

	printf("osb_err:%f\tdist_err:%f\tang_err:%f\tl_vel: %f\tr_vel:%f\n", obs_err, dis_err, ang_err, l_vel, r_vel);

	ang_err = (ang_err >=0 ? 1 : -1) * log(1 + fabs(ang_err));

	//顺时针转动为正  obs_err == 0 无障碍
	l_vel = pow(1 - fabs(obs_err), 2) * (fabs(ang_err) > 5.0 ? (ka * ang_err) : (kd * log10(1 + dis_err) + ka * ang_err)) + ko * obs_err;
	r_vel = pow(1 - fabs(obs_err), 2) * (fabs(ang_err) > 5.0 ? (ka * ang_err) : (kd * log10(1 + dis_err) - ka * ang_err)) - ko * obs_err;

	set_speed((int)r_vel, (int)l_vel);

	return 0;
}
